package com.example.blue;

import android.os.Environment;

import com.chaquo.python.PyObject;
import com.chaquo.python.Python;
import com.jmatio.io.MatFileReader;
import com.jmatio.types.MLArray;
import com.jmatio.types.MLStructure;

import org.example.DataProcessor;
import org.example.LibSvmModelLoader;
import org.example.orientationalgorithm.ArrayMatrixOperation;
import org.example.orientationalgorithm.ErrorState;
import org.example.orientationalgorithm.OrientationEstimation;
import org.example.orientationalgorithm.OrientationInputData;
import org.example.orientationalgorithm.OrientationOutputData;
import org.example.result.ZhenChanResult;

import java.io.BufferedReader;
import java.io.File;
import java.io.FileReader;
import java.io.IOException;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import java.util.Map;

import Jama.Matrix;
import libsvm.svm;
import libsvm.svm_model;
import libsvm.svm_node;

public class ZhenChan {
    public ZhenChan(){}
    public static DataProcessor dataProcessor = new DataProcessor();

    private static final String[] nameTableForClassify = {"angFreCV_R_PTT", "angF0_L_RT", "angPeakIQR_M_P",
            "angPeakAvg_T_P", "angF50_L_RT", "angPeakCV_W_RT",
            "angSF50_T_PT", "angF50_F_RT"};
    private static final String[] nameTableForRegressRT = {"angPhaseM2L_RT", "angPhaseW2R_RT", "angFreCV_RT",
            "angSF50_RT", "angPhaseW2F_RT", "angPeakAvg_RT"};
    private static final String[] nameTableForRegressP = {"angRotZ_P", "angPhaseF2L_P", "angK_P",
            "angPeakCV_P", "angFreIQR_P", "angF50_P"};
    private static final String[] nameTableForRegressPT = {"angK_PT", "angPeakAvg_PT", "angF0_PT",
            "angF50_PT"};
    private static final String[] nameTableForRegressPTT = {"angF50_PTT", "angRMS_PTT", "angPhaseT2L_PTT",
            "angPhaseF2R_PTT", "angPhaseR2L_PTT", "angPeakAvg_PTT"};

    private static final String[] nameTableForFinalRT = {"maxIdx_RT", "angPeakMax_RT", "angF0_RT"};
    private static final String[] nameTableForFinalP = {"maxIdx_P", "angPeakMax_P", "angF0_P"};
    private static final String[] nameTableForFinalPT = {"maxIdx_PT", "angPeakMax_PT", "angF0_PT"};
    private static final String[] nameTableForFinalPTT = {"maxIdx_PTT", "angPeakMax_PTT", "angF0_PTT"};

    private static int[] pca = new int[6];
    private static String[] combineArrays(String[]... arrays) {
        int totalLength = 0;
        for (String[] array : arrays) {
            totalLength += array.length;
        }

        String[] result = new String[totalLength];
        int currentIndex = 0;
        for (String[] array : arrays) {
            System.arraycopy(array, 0, result, currentIndex, array.length);
            currentIndex += array.length;
        }
        return result;
    }
    private static double[] combinedoubleArrays(double[]... arrays) {
        int totalLength = 0;
        for (double[] array : arrays) {
            totalLength += array.length;
        }

        double[] result = new double[totalLength];
        int currentIndex = 0;
        for (double[] array : arrays) {
            System.arraycopy(array, 0, result, currentIndex, array.length);
            currentIndex += array.length;
        }
        return result;
    }

    private static int findIndex(String target, String[] source) {
        for (int i = 0; i < source.length; i++) {
            if (target.equals(source[i])) {
                // 返回 Java 索引 (从 0 开始)
                return i;
            }
        }
        return -1; // 未找到
    }

    private static double[] getSubArray(double[] array, int[] indices) {
        double[] subArray = new double[indices.length];
        for (int i = 0; i < indices.length; i++) {
            subArray[i] = array[indices[i]];
        }
        return subArray;
    }

    public static double[] mapstd(double[] X, Map<String, double[]> PS) {
        double[] res = new double[X.length];
        double[] temp = PS.get("xoffset");
        for(int i = 0; i < temp.length; i++) {
            res[i] = X[i] - temp[i];
        }
        temp = PS.get("gain");
        for(int i = 0; i < temp.length; i++) {
            res[i] = temp[i] * res[i];
        }
        return res;
    }

    private svm_node[] createSvmNodes(double[] features) {
        svm_node[] nodes = new svm_node[features.length];
        for (int i = 0; i < features.length; i++) {
            nodes[i] = new svm_node();
            nodes[i].index = i + 1; // LIBSVM index starts from 1
            nodes[i].value = features[i];
        }
        return nodes;
    }

    public ZhenChanResult dataprocess(String filepath1, String filepath2, String filepath3, String filepath4) throws IOException {

        int fs = 100; //采样率
        int nTremor = 1500; //采样点个数

        ZhenChanResult res = new ZhenChanResult();

        List<double[][]> qua_test = new ArrayList<>();
        File file1 = new File(Environment.getExternalStoragePublicDirectory(
                Environment.DIRECTORY_DOCUMENTS), "qua1.txt");
        File file2 = new File(Environment.getExternalStoragePublicDirectory(
                Environment.DIRECTORY_DOCUMENTS), "qua2.txt");
        File file3 = new File(Environment.getExternalStoragePublicDirectory(
                Environment.DIRECTORY_DOCUMENTS), "qua3.txt");
        File file4 = new File(Environment.getExternalStoragePublicDirectory(
                Environment.DIRECTORY_DOCUMENTS), "qua4.txt");
        File file5 = new File(Environment.getExternalStoragePublicDirectory(
                Environment.DIRECTORY_DOCUMENTS), "qua5.txt");
        File file6 = new File(Environment.getExternalStoragePublicDirectory(
                Environment.DIRECTORY_DOCUMENTS), "qua6.txt");
        qua_test.add(readTxtToDoubleArray(file1.getAbsolutePath()));
        qua_test.add(readTxtToDoubleArray(file2.getAbsolutePath()));
        qua_test.add(readTxtToDoubleArray(file3.getAbsolutePath()));
        qua_test.add(readTxtToDoubleArray(file4.getAbsolutePath()));
        qua_test.add(readTxtToDoubleArray(file5.getAbsolutePath()));
        qua_test.add(readTxtToDoubleArray(file6.getAbsolutePath()));
        //zhenchanfea fea_test = alTremor(qua_test,"_test");

        //1.获取四个文件数据
        //1.1
        List<double[]> list_RT =  DataProcessor.datafromfile(filepath1);
        List<double[]> list_P = DataProcessor.datafromfile(filepath2);
        List<double[]> list_PT = DataProcessor.datafromfile(filepath3);
        List<double[]> list_PTT = DataProcessor.datafromfile(filepath4);
        //直接拿四元数
//        List<double[][]> qua_RT = calqua(list_RT);
//        List<double[][]> qua_P = calqua(list_P);
//        List<double[][]> qua_PT = calqua(list_PT);
//        List<double[][]> qua_PTT = calqua(list_PTT);


        //2.过滤异常值点，重新生成四元数
        List<double[]> filtered_RT = qua(filterOutlier(list_RT));
        List<double[]> filtered_P = qua(filterOutlier(list_P));
        List<double[]> filtered_PT = qua(filterOutlier(list_PT));
        List<double[]> filtered_PTT = qua(filterOutlier(list_PTT));

        //拿过滤后重新计算的四元数
        List<double[][]> qua_RT = calqua(filtered_RT);
        List<double[][]> qua_P = calqua(filtered_P);
        List<double[][]> qua_PT = calqua(filtered_PT);
        List<double[][]> qua_PTT = calqua(filtered_PTT);



        //3.特征计算
        zhenchanfea fea_RT = alTremor(qua_RT,"_RT");
        //zhenchanfea fea_RT = alTremor(qua_test,"_RT");
        zhenchanfea fea_P = alTremor(qua_P, "_P");
        zhenchanfea fea_PT = alTremor(qua_PT, "_PT");
        zhenchanfea fea_PTT = alTremor(qua_PTT, "_PTT");





        //4.svm分类
        //4.1提取指定特征
        String[] nameRT = fea_RT.getNames();
        String[] nameP = fea_P.getNames();
        String[] namePT = fea_PT.getNames();
        String[] namePTT = fea_PTT.getNames();
        String[] nameTotal = combineArrays(nameRT, nameP, namePT, namePTT);

        double[] feaRT = fea_RT.getFea();
        double[] feaP = fea_P.getFea();
        double[] feaPT = fea_PT.getFea();
        double[] feaPTT = fea_PTT.getFea();
        double[] feaTotal = combinedoubleArrays(feaRT, feaP, feaPT, feaPTT);

        //4.1.1提取最终要输出的特征
        int[] idxForFinalRT = new int[nameTableForFinalRT.length];
        int[] idxForFinalP = new int[nameTableForFinalP.length];
        int[] idxForFinalPT = new int[nameTableForFinalPT.length];
        int[] idxForFinalPTT = new int[nameTableForFinalPTT.length];


        for (int i = 0; i < nameTableForFinalRT.length; i++) {
            idxForFinalRT[i] = findIndex(nameTableForFinalRT[i], nameRT);
        }
        for (int i = 0; i < nameTableForFinalP.length; i++) {
            idxForFinalP[i] = findIndex(nameTableForFinalP[i], nameP);
        }
        for (int i = 0; i < nameTableForFinalPT.length; i++) {
            idxForFinalPT[i] = findIndex(nameTableForFinalPT[i], namePT);
        }
        for (int i = 0; i < nameTableForFinalPTT.length; i++) {
            idxForFinalPTT[i] = findIndex(nameTableForFinalPTT[i], namePTT);
        }

        double[] feafinalRT = getSubArray(feaRT, idxForFinalRT);
        double[] feafinalP = getSubArray(feaP, idxForFinalP);
        double[] feafinalPT = getSubArray(feaPT, idxForFinalPT);
        double[] feafinalPTT = getSubArray(feaPTT, idxForFinalPTT);

        res.setRTDisFea(feafinalRT);
        res.setPDisFea(feafinalP);
        res.setPTDisFea(feafinalPT);
        res.setPTTDisFea(feafinalPTT);

        int[] idxForClassify = new int[nameTableForClassify.length];
        int[] idxForRT = new int[nameTableForRegressRT.length];
        int[] idxForP = new int[nameTableForRegressP.length];
        int[] idxForPT = new int[nameTableForRegressPT.length];
        int[] idxForPTT = new int[nameTableForRegressPTT.length];


        for (int i = 0; i < nameTableForClassify.length; i++) {
            idxForClassify[i] = findIndex(nameTableForClassify[i], nameTotal);
        }
        for (int i = 0; i < nameTableForRegressRT.length; i++) {
            idxForRT[i] = findIndex(nameTableForRegressRT[i], nameRT);
        }
        for (int i = 0; i < nameTableForRegressP.length; i++) {
            idxForP[i] = findIndex(nameTableForRegressP[i], nameP);
        }
        for (int i = 0; i < nameTableForRegressPT.length; i++) {
            idxForPT[i] = findIndex(nameTableForRegressPT[i], namePT);
        }
        for (int i = 0; i < nameTableForRegressPTT.length; i++) {
            idxForPTT[i] = findIndex(nameTableForRegressPTT[i], namePTT);
        }

        double[] feaForClassify = getSubArray(feaTotal, idxForClassify);
        double[] feaForRT = getSubArray(feaRT, idxForRT);
        double[] feaForP = getSubArray(feaP, idxForP);
        double[] feaForPT = getSubArray(feaPT, idxForPT);
        double[] feaForPTT = getSubArray(feaPTT, idxForPTT);

        //4.2导入mat
        File matFile = new File(Environment.getExternalStoragePublicDirectory(
                Environment.DIRECTORY_DOCUMENTS), "zcmodelPara.mat");
        svm_model classifyModel;
        svm_model modelRT;
        svm_model modelP;
        svm_model modelPT;
        svm_model modelPTT;
        Map<String, double[]> classifyPS;
        Map<String, double[]> RTPS;
        Map<String, double[]> PPS;
        Map<String, double[]> PTPS;
        Map<String, double[]> PTTPS;
        MatFileReader matFileReader = null;
        try {
            matFileReader = new MatFileReader(matFile.getAbsolutePath());
        } catch (IOException e) {
            throw new RuntimeException(e);
        }
        Map<String, MLArray> content = matFileReader.getContent();

        MLArray classifyModelML = content.get("classifyModel");
        if (classifyModelML instanceof MLStructure) {
            classifyModel = LibSvmModelLoader.convertSvmModel((MLStructure) classifyModelML);
            System.out.println("Loaded Model1");
        } else {
            System.err.println("ERROR: 'classifyModel' not found or not an MLStructure.");
            classifyModel = null;
        }

        MLArray modelRTML = content.get("modelRT");
        if (modelRTML instanceof MLStructure) {
            modelRT = LibSvmModelLoader.convertSvmModel((MLStructure) modelRTML);
            System.out.println("Loaded Model1");
        } else {
            System.err.println("ERROR: 'classifyModel' not found or not an MLStructure.");
            modelRT = null;
        }

        MLArray modelPML = content.get("modelP");
        if (modelPML instanceof MLStructure) {
            modelP = LibSvmModelLoader.convertSvmModel((MLStructure) modelPML);
            System.out.println("Loaded Model1");
        } else {
            System.err.println("ERROR: 'classifyModel' not found or not an MLStructure.");
            modelP = null;
        }

        MLArray modelPTML = content.get("modelPT");
        if (modelPTML instanceof MLStructure) {
            modelPT = LibSvmModelLoader.convertSvmModel((MLStructure) modelPTML);
            System.out.println("Loaded Model1");
        } else {
            System.err.println("ERROR: 'classifyModel' not found or not an MLStructure.");
            modelPT = null;
        }

        MLArray modelPTTML = content.get("modelPTT");
        if (modelPTTML instanceof MLStructure) {
            modelPTT = LibSvmModelLoader.convertSvmModel((MLStructure) modelPTTML);
            System.out.println("Loaded Model1");
        } else {
            System.err.println("ERROR: 'classifyModel' not found or not an MLStructure.");
            modelPTT = null;
        }

        MLArray classifyPSML = content.get("classifyPS");
        if (classifyPSML instanceof MLStructure) {
            classifyPS = LibSvmModelLoader.convertPS((MLStructure) classifyPSML);
            System.out.println("Loaded PS");
        } else {
            System.err.println("ERROR: 'PS' not found or not an MLStructure.");
            classifyPS = null;
        }

        MLArray RTPSML = content.get("RTPS");
        if (RTPSML instanceof MLStructure) {
            RTPS = LibSvmModelLoader.convertPS((MLStructure) RTPSML);
            System.out.println("Loaded PS");
        } else {
            System.err.println("ERROR: 'PS' not found or not an MLStructure.");
            RTPS = null;
        }

        MLArray PPSML = content.get("PPS");
        if (PPSML instanceof MLStructure) {
            PPS = LibSvmModelLoader.convertPS((MLStructure) PPSML);
            System.out.println("Loaded PS");
        } else {
            System.err.println("ERROR: 'PS' not found or not an MLStructure.");
            PPS = null;
        }

        MLArray PTPSML = content.get("PTPS");
        if (PTPSML instanceof MLStructure) {
            PTPS = LibSvmModelLoader.convertPS((MLStructure) PTPSML);
            System.out.println("Loaded PS");
        } else {
            System.err.println("ERROR: 'PS' not found or not an MLStructure.");
            PTPS = null;
        }

        MLArray PTTPSML = content.get("PTTPS");
        if (PTTPSML instanceof MLStructure) {
            PTTPS = LibSvmModelLoader.convertPS((MLStructure) PTTPSML);
            System.out.println("Loaded PS");
        } else {
            System.err.println("ERROR: 'PS' not found or not an MLStructure.");
            PTTPS = null;
        }

        //4.2.? 归一化处理
        double[] normalizedFeaForClassify = mapstd(feaForClassify, classifyPS);
        double[] normalizedFeaForRT = mapstd(feaForRT, RTPS);
        double[] normalizedFeaForP = mapstd(feaForP, PPS);
        double[] normalizedFeaForPT = mapstd(feaForPT, PTPS);
        double[] normalizedFeaForPTT = mapstd(feaForPTT, PTTPS);

        svm_node[] classifyNodes = createSvmNodes(normalizedFeaForClassify);
        svm_node[] RTNodes = createSvmNodes(normalizedFeaForRT);
        svm_node[] PNodes = createSvmNodes(normalizedFeaForP);
        svm_node[] PTNodes = createSvmNodes(normalizedFeaForPT);
        svm_node[] PTTNodes = createSvmNodes(normalizedFeaForPTT);

        int predictLabel = (int) svm.svm_predict(classifyModel, classifyNodes);
        int RTLabel = (int) svm.svm_predict(modelRT, RTNodes);
        int PLabel = (int) svm.svm_predict(modelP, PNodes);
        int PTLabel = (int) svm.svm_predict(modelPT, PTNodes);
        int PTTLabel = (int) svm.svm_predict(modelPTT, PTTNodes);
        res.setPredictLabel(predictLabel);
        res.setRTScore(RTLabel);
        res.setPScore(PLabel);
        res.setPTScore(PTLabel);
        res.setPTTScore(PTTLabel);

        //返回结果
        return res;
    }
    public zhenchanfea alTremor(List<double[][]> qua, String addname){
        double fl = 3.0;
        double fh = 12.0;
        double fs = 100.0;
        double bw = 2;
        double nFFT = 128;
        double nOverlap = nFFT/2;
        Python py = Python.getInstance();
        PyObject pyModule = py.getModule("zhenchan");
        PyObject pyresult = pyModule.callAttr(
                "step1",
                qua.get(0),
                qua.get(1),
                qua.get(2),
                qua.get(3),
                qua.get(4),
                qua.get(5)
        );
        PyObject angP = pyresult.asList().get(0);        // numpy array
        PyObject angO = pyresult.asList().get(1);  // list of strings
        double[][] angP_java = angP.toJava(double[][].class);
        double[][] angO_java = angO.toJava(double[][].class);

        filterfea step4in = filterAnalyzeByPSD(angP_java, nFFT, nOverlap, fl, fh, fs, bw);
        PyObject pyresult4 = pyModule.callAttr(
                "step4", step4in.getF(), step4in.getF0(), step4in.getF50(), step4in.getSF50(), step4in.getK(), angO_java,
                qua.get(0), qua.get(1), qua.get(2), qua.get(3), qua.get(4), qua.get(5), addname
        );
        PyObject nameTable_py = pyresult4.asList().get(0);
        PyObject fea_py = pyresult4.asList().get(1);
        String[] nameTable = nameTable_py.toJava(String[].class);
        double[] fea = fea_py.toJava(double[].class);
        return new zhenchanfea(nameTable, fea);
    }

    public double[][] sensor_qua(List<double[]> list, int pos){
        double[][] res = new double[1500][4];
        int i = 0;
        for(double[] rawdata:list){
            System.arraycopy(rawdata, pos, res[i++], 0, 4);
        }
        return res;
    }

    public List<double[][]> calqua(List<double[]> list){
        List<double[][]> res = new ArrayList<>();
        res.add(sensor_qua(list, 54));
        res.add(sensor_qua(list, 58));
        res.add(sensor_qua(list, 62));
        res.add(sensor_qua(list, 66));
        res.add(sensor_qua(list, 70));
        res.add(sensor_qua(list, 74));
        return res;
    }

    public filterfea filterAnalyzeByPSD(double[][] angP_java, double nFFT, double nOverlap, double fl, double fh, double fs, double bw){
        double[][] F = new double[1500][6];
        double[] F0 = new double[6];
        double[] F50 = new double[6];
        double[] SF50 = new double[6];
        double[] K = new double[6];
        for(int i=0; i<6; i++){
            double[] v = new double[1500];
            for(int j = 0 ; j<1500;j++){
                v[j] = angP_java[j][i];
            }
            Python py = Python.getInstance();
            PyObject pyModule = py.getModule("zhenchan");
            PyObject pyresult = pyModule.callAttr(
                    "step2",
                    v,
                    nFFT,
                    nOverlap,
                    fl,
                    fh,
                    fs,
                    bw
            );
            //PyObject f0_py = pyresult.asList().get(0);
            double f0 = pyresult.toJava(double.class);
            double[] vF = DataProcessor.filterBandPass(v, f0 - bw, f0 + bw, fs);
            PyObject pyresult2 = pyModule.callAttr(
                    "step3",vF, nFFT, nOverlap, fs, fl, fh
            );
            PyObject F0_py = pyresult2.asList().get(0);
            PyObject F50_py = pyresult2.asList().get(1);
            PyObject SF50_py = pyresult2.asList().get(2);
            PyObject K_py = pyresult2.asList().get(3);

            double F0_s = F0_py.toJava(double.class);
            double F50_s = F50_py.toJava(double.class);
            double SF50_s = SF50_py.toJava(double.class);
            double K_s = K_py.toJava(double.class);
            for(int j = 0; j<1500;j++){
                F[j][i] = vF[j];
            }
            F0[i] = F0_s;
            F50[i] = F50_s;
            SF50[i] = SF50_s;
            K[i] = K_s;
        }
        return new filterfea(F,F0, F50,SF50,K);
    }

    public static double[][] readTxtToDoubleArray(String filePath) {
        List<double[]> tempRows = new ArrayList<>();

        try (BufferedReader br = new BufferedReader(new FileReader(filePath))) {
            String line;
            while ((line = br.readLine()) != null) {
                // 忽略空行
                if (line.trim().isEmpty()) {
                    continue;
                }

                // 使用空格作为分隔符
                String[] parts = line.trim().split("\\s+");
                double[] row = new double[parts.length];

                for (int i = 0; i < parts.length; i++) {
                    row[i] = Double.parseDouble(parts[i]);
                }
                tempRows.add(row);
            }
        } catch (IOException e) {
            e.printStackTrace();
            return null; // 出现错误时返回null
        }

        // 将 List<double[]> 转换为 double[][]
        double[][] result = new double[tempRows.size()][];
        for (int i = 0; i < tempRows.size(); i++) {
            result[i] = tempRows.get(i);
        }

        return result;
    }

    public static List<double[]> filterOutlier(List<double[]> raw) {
        if (raw == null || raw.isEmpty()) {
            throw new IllegalArgumentException("Input list cannot be null or empty");
        }
        int nRows = raw.size();
        int nCols = raw.get(0).length;

        // 深拷贝数据
        List<double[]> rawFiltered = new ArrayList<>();
        for (double[] row : raw) {
            rawFiltered.add(Arrays.copyOf(row, nCols));
        }

        double f = 10.0; // 阈值倍数

        for (int i = 0; i < nCols; i++) {
            // 提取第 i 列
            double[] col = new double[nRows];
            for (int j = 0; j < nRows; j++) {
                col[j] = raw.get(j)[i];
            }

            double rawSTD = std(col);
            double[] rawDiff = diff(col);
            double[] rawDiffFront = Arrays.copyOfRange(rawDiff, 0, rawDiff.length - 1);
            double[] rawDiffBehind = Arrays.copyOfRange(rawDiff, 1, rawDiff.length);

            double q75 = quantile(col, 0.75);
            double q25 = quantile(col, 0.25);
            double rawUpperBound = q75 + f * rawSTD;
            double rawLowerBound = q25 - f * rawSTD;

            for (int k = 1; k < nRows - 1; k++) {
                boolean exceedBound = (col[k] > rawUpperBound || col[k] < rawLowerBound);
                boolean largeFront = Math.abs(rawDiffFront[k - 1]) > rawSTD * f;
                boolean largeBehind = Math.abs(rawDiffBehind[k - 1]) > rawSTD * f;
                boolean opposite = rawDiffFront[k - 1] * rawDiffBehind[k - 1] < 0;

                if (exceedBound && largeFront && largeBehind && opposite) {
                    if (k - 1 >= 0 && k + 1 < nRows) {
                        rawFiltered.get(k)[i] = (col[k - 1] + col[k + 1]) / 2.0;
                    }
                }
            }
        }

        return rawFiltered;
    }

    // ====== 辅助函数 ======

    private static double std(double[] data) {
        double mean = Arrays.stream(data).average().orElse(0);
        double sumSq = 0;
        for (double v : data) {
            sumSq += Math.pow(v - mean, 2);
        }
        return Math.sqrt(sumSq / (data.length - 1));
    }

    private static double[] diff(double[] data) {
        double[] d = new double[data.length - 1];
        for (int i = 0; i < data.length - 1; i++) {
            d[i] = data[i + 1] - data[i];
        }
        return d;
    }

    private static double quantile(double[] data, double q) {
        double[] sorted = Arrays.copyOf(data, data.length);
        Arrays.sort(sorted);
        double pos = q * (sorted.length - 1);
        int idx = (int) pos;
        double frac = pos - idx;
        if (idx + 1 < sorted.length) {
            return sorted[idx] * (1 - frac) + sorted[idx + 1] * frac;
        } else {
            return sorted[idx];
        }
    }

    public List<double[]> qua(List<double[]> list){
        //新qua计算

        Matrix[] lastw = DataProcessor.lastw;
        Matrix[] lastQ = DataProcessor.lastQ;
        double timeStep=1.0/100;
        ErrorState[] errorState = DataProcessor.errorState;
        boolean[] initialFlag = DataProcessor.initialFlag;

        List<double[]> res = new ArrayList<>();
        for(double[] raw:list){
            double[] newdata = raw;
            for(int i = 0;i < 6;i++){
                double[] sensordata = new double[9];

                System.arraycopy(raw, 0+9*i, sensordata, 0, 9);
                OrientationInputData orientationInputData= new OrientationInputData(sensordata,lastQ[i],lastw[i],timeStep,errorState[i]);
                OrientationOutputData orientationOutputData;
                orientationOutputData= OrientationEstimation.oneSensorEstimation(orientationInputData,initialFlag[i],OrientationEstimation.AHRS,OrientationEstimation.quaternion);

                if(orientationOutputData!=null) {

                    if (!initialFlag[i]) initialFlag[i] = true;//初始化完成
                    lastQ[i] = orientationOutputData.getQua();
                    lastw[i] = orientationOutputData.getW();
                    errorState[i] = orientationOutputData.getErrorState();
                    double[] qua = new double[4];
                    double[] resultQua = ArrayMatrixOperation.transMatrixToLine(lastQ[i]);
                    System.arraycopy(resultQua, 0, newdata, 54+4*i, 4);
                }
            }
            res.add(newdata);
        }
        return res;
    }
}
